home *** CD-ROM | disk | FTP | other *** search
/ Aminet 7 / Aminet 7 - August 1995.iso / Aminet / misc / sci / RARS_Amiga_3.lha / RARS / burns.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-05-27  |  8.9 KB  |  196 lines
  1. // BURNS.CPP - "driver" function for RARS v0.50 
  2. // by Safai Ma, April 95, bg174@freenet.toronto.on.ca
  3. // This robot driver is very simple minded.  He will stay in the middle of
  4. // the track when he is on the straights and stay in the inside when
  5. // cornering. No code is done to handle any traffic though.  Whenever I
  6. // try to do it, it just slows him down :(  With this set of hand-optimized
  7. // parameters set, speed up is already possible by tweaking the parameters set.
  8. // (track specific parameter data files is not implemented yet :( this could
  9. // really speed he up)  Right now, he will stays on all the tracks that are
  10. // available on the ftp site. :)  Have fun!
  11.  
  12. //////////////////////////////  I N C L U D E S  ///////////////////////////////
  13.  
  14. #include <string.h>                                                // strcpy
  15. #include <math.h>                                                  // fabs, sqrt
  16.  
  17. #include "car.h"                                                   // stuck
  18.  
  19. ///////////////////////////////  D E F I N E S  ////////////////////////////////
  20.  
  21. #define ACCEL                   35.00     // acceleration
  22. #define BIAS                     0.06     // estimate of alpha when cornering
  23. #define BIG_SLIP                17.00     // affects the bias of steering servo
  24. #define CORN_SPD_CON             5.86     // determines how fast to take corners
  25. #define DECEL                   35.00     // braking deceleration
  26. #define DELTA_LANE              10.00     // when dead ahead, change lane
  27. #define END_CR                   1.40     // end of corners
  28. #define END_ST                   2.70     // end of straights
  29. #define MIN_CORN_SPD            52.00     // minimum cornering speed
  30. #define SHORT_CORN               0.60     // fraction of width considered short
  31. #define SHORT_CORN_SPD           5.60     // short corner speed constant
  32. #define SMALL_CURVE              1.10     // small curvature (rad/len)
  33. #define START_ACC                0.55     // fraction of seg. len. to start acc.
  34. #define STEER_DAMP               1.50     // servo damping, to prevent "weaving"
  35. #define STEER_GAIN               2.50     // servo gain, for staying in "lane"
  36. #define STRAIGHT_SPD           400.00     // speed for straights
  37.  
  38. ///////////////////////////////  E X T E R N S  ////////////////////////////////
  39.  
  40.  
  41. ////////////////////////////////  M A C R O S  /////////////////////////////////
  42.  
  43. #define max(x,y)                (((x)<(y))?(y):(x))            // return maximum
  44. #define min(x,y)                (((x)>(y))?(y):(x))            // return minimum
  45. #define sqr(x)                  ((x)*(x))                      // return x}
  46.  
  47. /////////////////////  S U P P O R T   F U N C T I O N S  //////////////////////
  48.  
  49. ////                                                                        ////
  50.  //     calculate cornering speed (funny name to avoid duplications)         //
  51. ////                                                                        ////
  52.  
  53. double burns_cs (double radius, double len, double lane, double width)
  54. {
  55.     double s;
  56.  
  57.     if (radius < 0.0)                     // change sign of negative radius
  58.     {
  59.         radius = - radius;                // make it positive
  60.         lane = width - lane;              // get the right lane for right turn
  61.     }
  62.     else if (radius == 0.0)               // return a high speed for straights
  63.         return (STRAIGHT_SPD);
  64.  
  65.     // calculate cornering speed
  66.     s = max (MIN_CORN_SPD, CORN_SPD_CON * sqrt (radius + lane));
  67.  
  68.     if (radius/len > SMALL_CURVE)         // for small curvature, increase speed
  69.         s += CORN_SPD_CON * sqrt (SHORT_CORN_SPD * radius / len);
  70.     else if (len < SHORT_CORN * width)    // for short corners, increase speed
  71.         s += CORN_SPD_CON * sqrt (SHORT_CORN_SPD * 7.0);
  72.  
  73.     return s;
  74. }                                                     // end of routine burns_cs
  75.  
  76. ////                                                                        ////
  77.  //     calculate segment length (for both curves and straights), in feet    //
  78. ////                                                                        ////
  79.  
  80. double burns_sl (double rad, double len, double width)
  81. {
  82.     if (rad != 0.0) return ((fabs (rad) + 0.5 * width) * len);
  83.     return (len);
  84. }                                                     // end of routine burns_sl
  85.  
  86. ////                                                                        ////
  87.  //     decides which lane to drive on (lane selection, wrt to left)         //
  88. ////                                                                        ////
  89.  
  90. double burns_ls (double rad, double width, double len)
  91. {
  92.     double lane;
  93.  
  94.     if (rad == 0.0) lane = .5 * width;
  95.     else if (len < .5 * width) lane = (rad > 0.0 ? .4 : .6) * width;
  96.     else if (rad < 0.0) lane = ((width < 100) ? (.75 * width) : (width - 25));
  97.     else if (rad > 0.0) lane = ((width < 100) ? (.25 * width) : (25));
  98.  
  99.     return lane;
  100. }                                                     // end of routine burns_ls
  101.  
  102. ////                                                                        ////
  103.  //     calculate braking distance using current and goal speed              //
  104. ////                                                                        ////
  105.  
  106. double burns_bd (double cur_v, double nex_v)
  107. {                                    // return -ve value if no braking is needed
  108.     return ((sqr(cur_v) - sqr(nex_v))/(2*DECEL));
  109. }                                                     // end of routine burns_bd
  110.  
  111. ///////////////////////  D R I V E R   F U N C T I O N  ////////////////////////
  112.  
  113. con_vec Burns (situation& s)
  114. {
  115.     const char name[] = "Burns";           // This is the robot driver's name!
  116.     static int init_flag = 1;              // cleared by first call
  117.     double alpha, vc;                      // components of result
  118.     con_vec result;                        // This is what is returned
  119.  
  120.     if (init_flag)                         // for first time only
  121.     {
  122.         my_name_is(name);          // copy name over
  123.         init_flag = 0;                     // reset init flag
  124.         result.alpha = result.vc = 0;
  125.         return result;
  126.     }
  127.  
  128.     // get the car out of an abnormal condition, thanks Mitchell :)
  129.     if (stuck (s.backward, s.v, s.vn, s.to_lft, s.to_rgt, &result.alpha, &result.vc))
  130.         return result;
  131.  
  132.     // get track width
  133.     double width = s.to_lft + s.to_rgt;
  134.  
  135.     // convert s.to_end to feets (for curves, don't want to work with radians :)
  136.     double to_end = burns_sl (s.cur_rad, s.to_end, width);
  137.  
  138.     // convert s.cur_len, s.nex_len to feets (for curves)
  139.     double cur_len = burns_sl (s.cur_rad, s.cur_len, width);
  140.     double nex_len = burns_sl (s.nex_rad, s.nex_len, width);
  141.  
  142.     // don't really need to convert this one but...just to make it consistent
  143.     // we don't have s.after_len :)
  144.     double aft_len = burns_sl (s.after_rad, s.after_rad, width);
  145.  
  146.     // select a lane for the current and the coming two track segments
  147.     double cur_lane = burns_ls (s.cur_rad, width, cur_len);
  148.     double nex_lane = burns_ls (s.nex_rad, width, nex_len);
  149.     double aft_lane = burns_ls (s.after_rad, width, aft_len);
  150.  
  151.     // convert a lane value to alpha (a standard procedure)
  152.     double cur_alpha = STEER_GAIN * (s.to_lft - cur_lane) / width;
  153.     double nex_alpha = STEER_GAIN * (s.to_lft - nex_lane) / width;
  154.  
  155.     // calculate intermediate alpha when changing lanes
  156.     double ratio = ((s.cur_rad == 0.0) ? END_ST : END_CR) * width;
  157.     if (ratio > cur_len) ratio = cur_len;
  158.     if (to_end < ratio)
  159.         alpha = cur_alpha*to_end/ratio + nex_alpha*(1-to_end/ratio);
  160.     else
  161.         alpha = cur_alpha;
  162.  
  163.     // calculate target speeds for the three track segments
  164.     double speed       = burns_cs (s.cur_rad,   cur_len, cur_lane, width);
  165.     double speed_next  = burns_cs (s.nex_rad,   nex_len, nex_lane, width);
  166.     double speed_after = burns_cs (s.after_rad, aft_len, aft_lane, width);
  167.  
  168.     // calculate braking distance for the next two track segments
  169.     double cur_brake_dist = burns_bd (s.v, speed_next);
  170.     double nex_brake_dist = burns_bd (s.v, speed_after);
  171.  
  172.     // now set the tire speed, vc
  173.     if ((to_end + nex_len) < nex_brake_dist)      // brake if neccessary
  174.         vc = speed_after;                         // for the 'after' corner
  175.     else if (to_end < cur_brake_dist)
  176.         vc = speed_next;                          // brake for next corner
  177.     else if ((to_end < (START_ACC * cur_len)) && (s.v < speed_next))
  178.         vc = speed_next;                          // accelerate for next segment
  179.     else
  180.         vc = speed;                               // keep current speed
  181.  
  182.     // calculate the bias (bias must be negative for right turn)
  183.     double bias = ((s.cur_rad!=0.0)?(sqr(s.v)/sqr(speed))*atan(BIG_SLIP/speed):0.0);
  184.     if (s.cur_rad < 0.0) bias = - bias;
  185.  
  186.     // adding bias and damping to prevent oscillation
  187.     alpha += bias - STEER_DAMP * s.vn / s.v ;
  188.  
  189.     // return calculated vc and alpha to host program
  190.     result.vc = vc;   result.alpha = alpha;
  191.     return result;
  192. }
  193.  
  194. //////////////////////  E N D   O F   B U R N S . C P P  ///////////////////////
  195.  
  196.